#include "rviz_planner/planner.hpp"
#include "thread"

namespace rviz_planner
{
    Planner::Planner(const rclcpp::NodeOptions &options) : nav2_util::LifecycleNode("rviz_planner", "", options)
    {
        RCLCPP_INFO(get_logger(), "Planner() create");
        // 创建路径发布者
        path_pub_ = this->create_publisher<nav_msgs::msg::Path>("/plan_path", 10);

        // 初始点位
        initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "/initialpose", 10, std::bind(&Planner::initial_pose_callback, this, std::placeholders::_1));
        // 目标点位
        goal_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/goal_pose", 10, std::bind(&Planner::goal_pose_callback, this, std::placeholders::_1));
        // 加载点位
        rclcpp::sleep_for(std::chrono::seconds(1));
        points_ = agv_util::load_waypoint();
        waypoint_neighbors_ = agv_util::load_waypoint_neighbors();
    }
    Planner::~Planner()
    {
    }
    nav2_util::CallbackReturn Planner::on_configure(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_configure");
        // marker
        rviz_marker_ptr_ = std::make_unique<rviz_util_pkg::RvizMarker>(shared_from_this());
        // 创建导航规划
        costmap_ = std::make_unique<rviz_planner::Costmap>();
        planner_ = std::make_unique<rviz_planner::Fixed>(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
        updateWaypoints();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn Planner::on_activate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_activate");
        createBond();
        publish_marker();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn Planner::on_deactivate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_deactivate");

        destroyBond();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn Planner::on_cleanup(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_cleanup");
        path_pub_.reset();
        rviz_marker_ptr_.reset();
        initial_pose_sub_.reset();
        goal_pose_sub_.reset();
        costmap_.reset();
        planner_.reset();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn Planner::on_shutdown(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_shutdown");
        return nav2_util::CallbackReturn::SUCCESS;
    }

    void Planner::initial_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
    {
        start_pose_ = msg->pose.pose;
        RCLCPP_INFO(get_logger(), "init_pose x=%f,y=%f,ori=%f,%f,%f,%f", start_pose_.position.x, start_pose_.position.y,
            start_pose_.orientation.x,
            start_pose_.orientation.y,
            start_pose_.orientation.z,
            start_pose_.orientation.w);
        double x = msg->pose.pose.position.x;
        double y = msg->pose.pose.position.y;
        rviz_marker_ptr_->clear_ns_points("start");
        rviz_marker_ptr_->publish_marker_point(x, y, "start");
        calculate_radian();
    }
    void Planner::goal_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
        goal_pose_ = msg->pose;
        RCLCPP_INFO(get_logger(), "goal_pose x=%f,y=%f", goal_pose_.position.x, goal_pose_.position.y);
        double x = msg->pose.position.x;
        double y = msg->pose.position.y;
        rviz_marker_ptr_->clear_ns_points("goal");
        rviz_marker_ptr_->publish_marker_point(x, y, "goal");
        publish_path();
        calculate_radian();
    }

    void Planner::publish_marker()
    {
        rclcpp::WallRate rate(10); // hz
        for (auto &point : points_)
        {
            double x = point.pose.position.x;
            double y = point.pose.position.y;
            // 发布
            rviz_marker_ptr_->publish_marker_point(x, y, point.name);
            rviz_marker_ptr_->publish_marker_txt(x, y, point.name);
            rate.sleep();
        }

        // for (auto &waypoint : points_)
        // {
        //     // 起始点
        //     double x1 = waypoint.pose.position.x;
        //     double y1 = waypoint.pose.position.y;
        //     auto neighbor_points = get_neighbor_nodes_by_name(waypoint.name);
        //     // 能通过的临近点集合

        //     for (auto &neighbor : neighbor_points)
        //     {
        //         double x2 = neighbor.pose.position.x;
        //         double y2 = neighbor.pose.position.y;
        //         rviz_marker_ptr_->publish_marker_line(x1, y1, x2, y2);
        //         rate.sleep();
        //     }
        // }
    }

    std::vector<nav_msg_pkg::msg::Waypoint> Planner::get_neighbor_nodes_by_name(std::string name)
    {
        // 检查 waypoint_neighbors_ 和 points_ 是否为空
        if (waypoint_neighbors_.empty() || points_.empty())
        {
            RCLCPP_WARN(get_logger(), "Empty waypoint_neighbors_ or points_");
            return {};
        }
        auto start_time = std::chrono::high_resolution_clock::now();
        std::vector<std::string> neighbor_names;
        for (auto &waypointNeighbor : waypoint_neighbors_)
        {
            if (waypointNeighbor.name == name)
            {
                neighbor_names = waypointNeighbor.neighbors;
                break;
            }
        }
        std::vector<nav_msg_pkg::msg::Waypoint> neighbor_nodes;
        for (auto &waypoint : points_)
        {
            if (std::find(neighbor_names.begin(), neighbor_names.end(), waypoint.name) != neighbor_names.end())
            {
                neighbor_nodes.push_back(waypoint);
            }
        }
        auto end_time = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
        // RCLCPP_INFO(get_logger(), "get_neighbor_nodes_by_name time= %ld us", duration.count());
        return neighbor_nodes;
    }

    void Planner::publish_path()
    {
        nav_msgs::msg::Path path; // 路径
        path.poses.clear();
        path.header.frame_id = "map";
        path.header.stamp = get_clock()->now();
        path_pub_->publish(path);

        double wx_start = start_pose_.position.x;
        double wy_start = start_pose_.position.y;
        unsigned int mx_start, my_start;
        if (!costmap_->worldToMap(wx_start, wy_start, mx_start, my_start))
        {
            RCLCPP_WARN(get_logger(), "无法规划路径，机器人起始位置不在全局代价地图上。");
            return;
        }
        planner_->setMapSize(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
        // 起始点
        int map_start[2];
        map_start[0] = mx_start;
        map_start[1] = my_start;
        // 目标点
        double wx_goal = goal_pose_.position.x;
        double wy_goal = goal_pose_.position.y;
        unsigned int mx_goal, my_goal;
        if (!costmap_->worldToMap(wx_goal, wy_goal, mx_goal, my_goal))
        {
            RCLCPP_WARN(get_logger(), "无法规划路径，目标点不在全局代价地图上。");
            return;
        }
        int map_goal[2];
        map_goal[0] = mx_goal;
        map_goal[1] = my_goal;
        planner_->setStart(map_start);
        planner_->setGoal(map_goal);
        auto node_list = planner_->searchPath();
        if (node_list == nullptr || node_list->empty())
        {
            RCLCPP_WARN(get_logger(), "node_list is empty。");
            return;
        }
        // 将节点生成路径
        for (size_t index = 0; index < node_list->size() - 1; index++)
        {
            auto node_ptr = node_list->at(index);
            auto next_node_ptr = node_list->at(index + 1);
            // 循环次数
            double loop_count = std::hypot(node_ptr->x - next_node_ptr->x, node_ptr->y - next_node_ptr->y);

            double x_increment = (next_node_ptr->x - node_ptr->x) / loop_count;
            double y_increment = (next_node_ptr->y - node_ptr->y) / loop_count;
            // RCLCPP_INFO(get_logger(), "x=%d,y=%d,next x=%d,y=%d", node_ptr->x, node_ptr->y,
            //             next_node_ptr->x, next_node_ptr->y);
            // RCLCPP_INFO(get_logger(), "循环次数:%f,x_increment:%f,y_increment:%f", loop_count, x_increment, y_increment);
            unsigned int map_x, map_y;
            double world_x, world_y; // 将栅格坐标转换为世界坐标
            for (int i = 0; i < loop_count; i++)
            {
                map_x = node_ptr->x + i * x_increment;
                map_y = node_ptr->y + i * y_increment;
                costmap_->mapToWorld(map_x, map_y, world_x, world_y);
                // 生成pose
                geometry_msgs::msg::PoseStamped pose;
                pose.header.frame_id = "map";
                pose.header.stamp = get_clock()->now();
                pose.pose.position.x = world_x;
                pose.pose.position.y = world_y;
                pose.pose.position.z = 0.0;
                pose.pose.orientation.x = 0.0;
                pose.pose.orientation.y = 0.0;
                pose.pose.orientation.z = 0.0;
                pose.pose.orientation.w = 1.0;
                path.poses.push_back(pose);
            }
        }
        path_pub_->publish(path);
        RCLCPP_INFO(get_logger(), "路径已发布");
    }

    bool Planner::updateWaypoints()
    {
        if (points_.empty())
        {
            return false;
        }
        if (!costmap_)
        {
            RCLCPP_ERROR(get_logger(), "costmap is not initialized");
            return false;
        }
        auto point_nodes = std::make_shared<std::vector<std::shared_ptr<rviz_planner::PointNodeFixed>>>();
        for (auto &point : points_)
        {
            unsigned int mx, my;
            if (!costmap_->worldToMap(point.pose.position.x, point.pose.position.y, mx, my))
            {
                RCLCPP_WARN(get_logger(), "无法规划路径，机器人路点 x=%f,y=%f 不在全局代价地图上。",
                            point.pose.position.x, point.pose.position.y);
                return false;
            }
            auto point_node = std::make_shared<rviz_planner::PointNodeFixed>(
                static_cast<int>(mx),
                static_cast<int>(my),
                nullptr,
                0,
                0,
                point.name);
            point_nodes->push_back(point_node);
        }
        planner_->set_node_list(point_nodes);
        return true;
    }

    void Planner::calculate_radian(){
        //以Z轴为轴心，X轴为起始点，逆时针旋转，[-π,π]
        double yaw_start = tf2::getYaw(start_pose_.orientation);
        double yaw_goal = tf2::getYaw(goal_pose_.orientation);
        double target_rad = std::atan2(goal_pose_.position.y - start_pose_.position.y, goal_pose_.position.x - start_pose_.position.x);
        double diff_rad = target_rad - yaw_start;
        RCLCPP_INFO(get_logger(), "yaw_start=%f,%f,atan2=%f,%f", yaw_start,yaw_goal,target_rad, diff_rad);
        // yaw_start=-0.009412,-0.776289,-0.809403,-0.799991
        double degree_start = yaw_start*(180.0/M_PI);
        double degree_goal = yaw_goal*(180.0/M_PI);
        double degree_diff = diff_rad*(180.0/M_PI);
        RCLCPP_INFO(get_logger(), "start=%f,goal=%f,diff=%f", degree_start,degree_goal,degree_diff);
        //第一象限
        // 弧度差=atan2 - yaw_start
    }
    
}   